####################################### 测试制导律 ######################################
# import math
# yaw_rad = 180*math.pi/180
# distance_to_line = -5
#
# hangdian2_x =0
# hangdian2_y=10
# k = 0.2
# print(math.degrees(math.atan(k * distance_to_line)) ,math.degrees(math.atan2(hangdian2_y, hangdian2_x)))
# ang_ = -math.atan(k * distance_to_line) + math.atan2(hangdian2_y, hangdian2_x)
#
# print(math.degrees(ang_))
#
# # 计算ang_和yaw_rad的夹角，限制在-pi到pi之间
# angle_error = ang_ - yaw_rad
# print(math.degrees(angle_error))
# # 将角度误差调整到 -π 到 π 之间
# angle_error = (angle_error + math.pi) % (2 * math.pi)-math.pi
#
# print(math.degrees(angle_error))




############################ 测试移动位置 ###############################

import ADRC

ADRC.rospy.init_node('teleport_boat_node')
target_point = [-400, 150]  # 目标点的 (x, y) 坐标
ADRC.goto_point(target_point)